package com.hitqz.robot.driver.util;

import cn.hutool.core.collection.CollUtil;
import cn.hutool.core.util.StrUtil;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import lombok.extern.slf4j.Slf4j;
import org.jgrapht.GraphPath;
import org.jgrapht.alg.shortestpath.DijkstraShortestPath;
import org.jgrapht.graph.DefaultDirectedWeightedGraph;
import org.jgrapht.graph.DefaultWeightedEdge;

import java.util.*;

/**
 * Dijkstra路径规划工具类
 */
@Slf4j
public class JgraphtUtil {
    //Dijkstra 最短路径算法的实例，该实例将用于计算图 graph 中的最短路径
    private static DijkstraShortestPath<String, DefaultWeightedEdge> dijkstraShortestPath = null;
    //code-RobotMapPos集合
    private static final Map<String, RobotMapPos> mapPosMap = new HashMap<>();

    /**
     * 初始化Dijkstra 最短路径算法的实例
     *
     * @param reInit   是否清缓存
     * @param posPaths 所有参与计算的路径集合
     * @return true 初始化成功，false 初始化失败
     */
    public static boolean initDijkstra(boolean reInit, List<RobotMapPos> posPaths) {
        if (dijkstraShortestPath == null || reInit) {
            if (posPaths == null || posPaths.isEmpty()) {
                log.warn("[Dijkstra]初始化失败:参与路径计算的集合为空");
                return false;
            }
            DefaultDirectedWeightedGraph<String, DefaultWeightedEdge> graph = new DefaultDirectedWeightedGraph<>(DefaultWeightedEdge.class);
            // 封装所有路径集合
            for (RobotMapPos posPath : posPaths) {
                mapPosMap.put(posPath.getCode(), posPath);
                if (StrUtil.isEmpty(posPath.getPreCode())) {
                    continue;
                }
                String[] preCodes = posPath.getPreCode().split(",");
                for (String preCode : preCodes) {
                    RobotMapPos prePos = mapPosMap.get(preCode);
                    graph.addVertex(preCode);
                    graph.addVertex(posPath.getCode());
                    DefaultWeightedEdge edge1 = graph.addEdge(preCode, posPath.getCode());
                    if (edge1 != null) {
                        //两点的直线距离(x1,y1) (x2,y2)
                        //fixme 二维码的情况要进行优化
                        double weight = Math.sqrt(Math.pow(posPath.getPosX() - prePos.getPosX(), 2) + Math.pow(posPath.getPosY() - prePos.getPosY(), 2));
                        graph.setEdgeWeight(edge1, weight);
                    }
                    // todo 反向
//                        if (posPath.getReversePath() != null && posPath.getReversePath() == 1) {
//                            graph.addVertex(posPath.getCode());
//                            graph.addVertex(preCodeArr[i]);
//                            DefaultWeightedEdge edge2 = graph.addEdge(posPath.getCode(), preCodeArr[i]);
//                            if (edge2 != null) {
//                                graph.setEdgeWeight(edge2, posPath.getWeight());
//                            }
//                        }
                }
            }
            dijkstraShortestPath = new DijkstraShortestPath<>(graph);
        }
        return true;
    }

    /**
     * 获取最短路径
     *
     * @param sourceCode 起始点
     * @param targetCode 目标点
     * @param posPaths   所有参与计算的路径集合
     * @return 最短路径集合
     */
    public static List<RobotMapPos> getShortestPath(String sourceCode, String targetCode, List<RobotMapPos> posPaths) {
        if (!initDijkstra(false, posPaths)) {
            return new ArrayList<>();
        }
        try {
            GraphPath<String, DefaultWeightedEdge> path = dijkstraShortestPath.getPath(sourceCode, targetCode);
            if (path == null) {
                log.warn("[Dijkstra] 没有找到从 {} 到 {} 的路径", sourceCode, targetCode);
                return new ArrayList<>();
            }
            // 构建路径字符串
            StringBuilder pathBuilder = new StringBuilder();
            for (String vertex : path.getVertexList()) {
                if (!pathBuilder.isEmpty()) {
                    pathBuilder.append("->");
                }
                pathBuilder.append(vertex);
            }
            String pathString = pathBuilder.toString();
            // 打印路径字符串
            log.info("[Dijkstra]最短路径: {}", pathString);
            List<RobotMapPos> shortestPath = new ArrayList<>();
            for (String vertex : path.getVertexList()) {
                RobotMapPos pos = mapPosMap.get(vertex);
                if (pos != null) {
                    shortestPath.add(pos);
                }
            }
            return shortestPath;
        }catch (Exception e){
            log.error("[Dijkstra]获取最短路径失败",e);
            return new ArrayList<>();
        }
    }


}
